.TITLE MOVE2

;CONTINUATION OF RUN-TIME ROUTINES.

;DEFINED GLOBALS

.GLOBL DRAW,MOVE,DEPART,APPRO,READY,REST,JOINT

;REQUIRED GLOBALS

.GLOBL UPDATE,SOLVE,TYPERR,CANPRO,ARMS,RDYTRN,RDYASS
.GLOBL NSPEED,CONFIG,RSTTRN,RSTASS,DRIVE,JANGLE,TIMER
.GLOBL EVAL,NOSOL,ARMDNE,ARMBRK,ONPATH
.GLOBL TTRANS,MARG,OUTRNG,TRNPTR,NOTDAT

;REGISTER DEFINITIONS

R0	=%0
R1	=%1
R2	=%2
R3	=%3
R4	=%4
R5	=%5
SP	=%6
PC	=%7
;"DRAW"   - RELATIVE ARM MOTION USING STRAIGHT LINE MOTION

;THE DRAW INSTRUCTION PERFORMS A RELATIVE CHANGE IN THE POSITION OF
;THE HAND OF THE ARM USING STRAIGHT LINE MOTION.  IT REQUIRES AS ITS
;ARGUMENTS THE CHANGE IN X,Y, AND Z TO BE MADE DURING THE MOTION.
;ITS MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		| CHANGE IN X DIRECTION |
;		|-----------------------|
;		| CHANGE IN Y DIRECTION |
;		|-----------------------|
;		| CHANGE IN Z DIRECTION |
;		|-----------------------|
;
;THE TOTAL MOTION TIME IS SET EQUAL TO THE TIME REQUIRED BY THE
;SLOWEST JOINT TO COMPLETE ITS CHANGE IN JOINT ANGLE.

DRAW:	MOV	R0,-(SP)	;SAVE PTR TO NEXT STEP
	ADD	#MARG,R1	;SAVE ADDR OF RELATIVE MOTIONS
	MOV	R1,R4
	MOV	R1,@#RELXYZ
	MOV	#TTRANS,R0	;GET CURRENT ARM TRANSFORM
	MOV	#JANGLE,R1
	JSR	PC,UPDATE	
	MOV	#TTRANS+22,R0	;FINAL XYZ ← PRESENT XYZ + RELXYZ
	MOV	#FINXYZ,R2
	MOV	#OUTRNG,R1	;FETCH ERROR MESSAGE,JUST IN CASE
	MOV	#3,R3
FINLOP:	ADD	(R4)+,(R0)
	BVS	DRWERR		;OVERFLOW?
	MOV	(R0)+,(R2)+
	SOB	R3,FINLOP
	MOV	#TTRANS,R0	;COMPUTE MOTION TIME
	MOV	#DRWANG,R1	;NEED FINAL JOINT ANGLES
	MOV	#ONPATH,R2	;MAINTAIN PRESENT CONFIGURATION
	JSR	PC,SOLVE
	TST	R0		;INVALID SOLUTION?
	BNE	DRWER2		;BRANCH IF NO SOLUTION
	MOV	#DRWANG,R1	;GET CHANGE IN JOINT ANGLES
	MOV	#JANGLE,R2
	MOV	#6,R3
	SUB	(R2)+,(R1)+
	SOB	R3,.-2
	MOV	#DRWANG,R0	;COMPUTE MOTION TIME
	JSR	PC,TIMER 
	MOV	R0,@#DRWTME	;SAVE IT
	ADD	#375.,R0	;+ NULLING TIME
	BVC	.+6
	MOV	#77777,R0	;USE MAX TIME IF OVERFLOW
	MOV	R0,@#DRWTTM	;TIME OUT TIME
	MOV	#DRWDAT,R1	;GO PERFORM THE ARM MOTION
	JSR	PC,DRIVE
	BR	DRWDNE

DRWER2:	BIS	#NOSOL,R0	;INDICATE ERROR
	MOV	R0,R1
DRWERR:	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

DRWDNE:	MOV	(SP)+,R0
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 16 MILLISECONDS

DRWSUB:	MOV	R1,-(SP)	;SAVE PTR TO JOINT ANGLES
	MOV	@#DRWTME,R2	;EVALUATE PERCENT CHANGE IN XYZ
	JSR	PC,EVAL
	MOV	R0,-(SP)	;COMPUTE NEW XYZ
	MOV	@#RELXYZ,R0	;NEW XYZ ← % CHANGE*RELXYZ+FINXYZ
	MOV	#FINXYZ,R1
	MOV	#TTRANS+22,R4
	MOV	#3,R5
NEWXYZ:	MOV	(R0)+,R2	;CHANGE IN X, Y, OR Z
	MUL	(SP),R2
	ASHC	#2,R2
	ADD 	(R1)+,R2 	;+ FINAL XYZ
	MOV	R2,(R4)+	;NEW XYZ
	SOB	R5,NEWXYZ
	TST	(SP)+
	MOV	#TTRANS,R0	;COMPUTE NEW JOINT SET POINTS
	MOV	(SP)+,R1	;SEND BACK IN HERE
	MOV	#ONPATH,R2	;MAINTAIN PRESENT CONFIGURATION
	JSR	PC,SOLVE
	RTS	PC


;LOCAL STORAGE AREA

DRWDAT:	ARMDNE	;ARM IN RANGE MASK BITS
DRWTTM:	0	;TIME OUT TIME
	DRWSUB	;SUBROUTINE TO OUTPUT INCREMENTAL MOTION
	ARMBRK	;BRAKE BITS
	1	;FIRST JOINT TO BE SERVOED
	6	;TOTAL NUMBER OF JOINTS TO BE SERVOED

RELXYZ:	0	;PTR TO RELATIVE CHANGE IN X,Y,Z
FINXYZ:	.=.+6	;FINAL X,Y,Z VALUES
DRWANG:	.=.+12.	;TEMPORARY STORAGE FOR JT ANGLES
DRWTME:	0	;MOTION TIME

;END OF "DRAW"
;"MOVE"   - ARM MOTION USING JOINT INTERPOLATION

;THE MOVE INSTRUCTION MOVES THE ARM TO A SPECIFIED LOCATION AND
;ORIENTATION USING JT. ANGLE INTERPOLATION TO DETERMINE INTERMEDIATE
;SET POINTS.  ITS MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		| ADDR OF TRANS SYM BLK |
;		|-----------------------|
;
;THE TOTAL MOTION TIME IS SET EQUAL TO THE TIME REQUIRED BY THE
;SLOWEST JOINT TO COMPLETE ITS CHANGE IN JOINT ANGLE.

MOVE: 	MOV	MARG(R1),R1	;TRANSFORM SYMBOL BLOCK
	MOV	TRNPTR(R1),R1	;TRANSFORM DATA?
	BEQ	MOVNT
	JSR	PC,JOINT	;GO DRIVE THE ARM
	BR	MOVDN

MOVNT:	MOV	#NOTDAT,R1	;INDICATE ERROR
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

MOVDN:	RTS	PC


;END OF "MOVE"
;"DEPART" - RELATIVE MOTION ALONG ARM'S -Z AXIS

;THIS INSTRUCTION MOVES THE ARM A SPECIFIED DISTANCE ALONG ITS OWN
;-Z AXIS RELATIVE TO THE ARMS CURRENT POSITION.  ITS MOTION STEP
;VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|       DISTANCE        |
;		|-----------------------|
;

DEPART:	MOV	R0,-(SP)	;SAVE PTR TO NEXT STEP
	MOV	MARG(R1),R4	;SAVE -DISTANCE TO CHANGE
	NEG	R4
	MOV	#TTRANS,R0	;GET CURRENT ARM TRANSFORM
	MOV	#JANGLE,R1
	JSR	PC,UPDATE	

RELMVE:	MOV	#TTRANS+14,R0	;NEW XYZ ← OLD XYZ + DIS*Z VECT
	MOV	#TTRANS+22,R1
	MOV	#3,R5
DEPLOP:	MOV	(R0)+,R2	;COMPONENT OF Z VECTOR
	MUL	R4,R2		; x DISTANCE
	ASHC	#2,R2		;NORMALIZE AND ROUND
	TST	R3
	BPL	.+4
	INC	R2
	ADD	R2,(R1)+	;CORRECT OLD XYZ
	BVS	DEPERR		;OVERFLOW?
	SOB	R5,DEPLOP
	MOV	#TTRANS,R1	;GO TO THIS NEW POSITION
	JSR	PC,JOINT
	BR	DEPDNE

DEPERR:	MOV	#OUTRNG,R1	;INDICATE ERROR
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

DEPDNE:	MOV	(SP)+,R0
	RTS	PC


;END OF "DEPART"
;"APPRO" - MOTION RELATIVE TO A TRANSFORM ALONG ARM'S -Z AXIS

;THIS INSTRUCTION MOVES THE ARM A SPECIFIED DISTANCE RELATIVE TO A
;DEFINED TRANSFORM LOCATION.  ITS MOTION VARIABLE STORAGE AREA
;LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   TRANSFORM POINTER   |
;		|-----------------------|
;		|       DISTANCE        |
;		|-----------------------|
;

APPRO: 	MOV	R0,-(SP)	;SAVE PTR TO NEXT STEP
	MOV	MARG+2(R1),R4	;SAVE -DISTANCE TO CHANGE
	NEG	R4
	MOV	MARG(R1),R0	;TRANSFORM TO APPROACH
	MOV	TRNPTR(R0),R0	;TRANSFORM DATA?
	BEQ	APRONT
	MOV	#TTRANS,R1	;COPY TRANS INTO HERE
	MOV	#12.,R3
	MOV	(R0)+,(R1)+
	SOB	R3,.-2
	JMP	RELMVE		;JUST LIKE A DEPART FROM THIS POINT

APRONT:	MOV	#NOTDAT,R1	;INDICATE ERROR
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS
	MOV	(SP)+,R0
	RTS	PC


;END OF "APPRO"
;"READY"&"REST" - MOVES THE ARM TO THE READY&REST POSITIONS

;THESE INSTRUCTIONS MOVE THE ARM TO THE PRE-DEFINED READY AND
;REST POSITIONS.  THEY REQUIRE NO ARGUMENTS.


READY:	MOV	#RDYTRN,R1	;READY POSITION
	MOV	@#RDYASS,@#CONFIG
	JMP	JOINT

REST:	TST	@#NSPEED	;USER REQUESTED SPECIAL SPEED?
	BNE	.+10
	MOV	#2000,@#NSPEED	;NO, MORE TO REST AT HALF SPEED
	MOV	#RSTTRN,R1	;REST POSITION
	MOV	@#RSTASS,@#CONFIG
	JMP	JOINT


;END OF "READY"&"REST"
;"JOINT"  - SUBR FOR GENERATED SET POINTS BY JOINT INTERPOLATION

;THE FOLLOWING SUBROUTINE CAN BE USED TO GENERATE A SMOOTH PATH BY
;INTERPOLATING BETWEEN THE CURRENT ARM POSITION AND A FINAL
;TRANSFORM.  IT REQUIRES AS ITS ONLY ARGUMENT, A POINTER TO A
;TRANSFORM.  A SAMPLE CALLING SEQUENCE FOLLOW:
;
;		MOV	#TRANS,R1
;		JSR	PC,JOINT

;REGISTERS USED:
;	R1 PASSES ARGUMENTS AND IS MODIFIED


JOINT:	MOV	R1,@#JTTRAN	;SAVE TRANS POINTER
	MOV	#375.,@#JTTIME	;NEED AT LEAST 3 SEC FOR MOTION
	CLR	@#JTPAS1	;FIRST LOOP THOUGH
	MOV	#JTDATA,R1	;"JOINT" SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE ARM JOINTS
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 16 MILLISECONDS

JOINTS:	MOV	R1,R5		;SAVE PTR TO JOINT ANGLES
	TST	@#JTPAS1	;NEED TO COMPUTE FINAL SET POINTS?
	BNE	JTINTP		;NO
	MOV	R0,-(SP)	;YES
	MOV	@#JTTRAN,R0	;CONVERT TRANS TO NEW JOINT ANGLES
	MOV	#FANGLE,R1
	MOV	@#CONFIG,R2	;ASSERT ANY REQUESTED CHANGES IN CONF
	CLR	@#CONFIG
	JSR	PC,SOLVE
	TST	R0		;INVALID SOLUTION?
	BNE	JTERR 		;BRANCH IF NO SOLUTION
	MOV	#FANGLE,R1	;GET CHANGE IN JOINT ANGLES
	MOV	#JANGLE,R2
	MOV	#DANGLE,R3	;SAVE IN HERE
	MOV	#6,R4
DIFANG:	MOV	(R1)+,R0
	SUB	(R2)+,R0
	MOV	R0,(R3)+
	SOB	R4,DIFANG
	MOV	#DANGLE,R0	;COMPUTE MOTION TIME
	JSR	PC,TIMER 
	MOV	R0,@#INTTME	;SAVE IT
	ADD	#375.,R0	;+ NULLING TIME
	MOV	R0,@#JTTIME	;TIME OUT TIME
	INC	@#JTPAS1
	MOV	(SP)+,R0
	MOV	R5,R1

JTINTP:	MOV	@#INTTME,R2	;EVALUATE PERCENT CHANGE IN SET POINT
	JSR	PC,EVAL
	MOV	R0,-(SP)	;COMPUTE NEW SET POINTS
	MOV	#DANGLE,R0	;NEW JT ANG ← % CHANGE*GANGC+GANGF
	MOV	#FANGLE,R1
	MOV	#6,R4
FINTLP:	MOV	(R0)+,R2	;CHANGE IN ANGLE
	MUL	(SP),R2
	ASHC	#2,R2
	ADD 	(R1)+,R2 	;+ FINAL SET POINT
	MOV	R2,(R5)+	;SAVE NEW SET POINT
	SOB	R4,FINTLP
	BR	JTITDN

JTERR: 	BIS 	#NOSOL,R0	;INVALID SOLUTION
	MOVB	R0,@#ARMS
	BIS	#CANPRO,@#ARMS

JTITDN:	TST	(SP)+
	RTS	PC


;LOCAL STORAGE AREA

JTDATA:	ARMDNE	;MASK BITS FOR ARM JOINTS
JTTIME:	0	;TIME OUT TIME
	JOINTS	;16MSEC SUBROUTINE
	ARMBRK	;BRAKE BITS
	1	;FIRST JOINT TO BE SERVOED
	6	;TOTAL NUMBER OF JOINTS TO BE SERVOED

JTPAS1:	0	;FIRST PASS THROUGH JOINTS?
JTTRAN:	0	;TRANS POINTER
INTTME:	0	;MOTION TIME
FANGLE:	.=.+12.	;FINAL JOINT ANGLES
DANGLE:	.=.+12.	;CHANGE IN JOINT ANGLES


;END OF "JOINT"

.END